/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file mag.cxx
	\brief class implemenation for the HMR3400 Magnetometer

                    \author Maciej Stachura
             $Date: 2010/03/03 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "mag.h"

  /*<-------Defines--------->*/
  #define MAG_SEND_PERIOD 1     // seconds
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
  /*<-End Global Variables-->*/

Mag::Mag(String name) : ThreadedPipe(name)
{
  buf_ptr = 0;
  mag.magnetic_heading = 0;
  mag.true_heading = 0;
  mag.pitch = 0;
  mag.roll = 0;
  new_meas = false;
  last_mag_tx.stamp();
}

void Mag::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;
  const char * mag_str;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("Mag::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {
        int start_ptr = 0;
        if(buf_ptr == 0) 
          for(; start_ptr<n; start_ptr++)
            if (fifo_msg.data[start_ptr] == '\n')
              break;

        if(buf_ptr + n > MAG_MEAS_BUF_SIZE)
          buf_ptr = 0;
        else {
          memcpy(&(buf[buf_ptr]), &(fifo_msg.data[start_ptr]), n-start_ptr);
          buf_ptr += n-start_ptr;

          int size = 0;
          for(; size < buf_ptr; size++){
            if((buf[size] == '\n') && (size > 12))
            {
              buf_ptr = 0;
              mag_str = (const char *) buf;
              sscanf(mag_str, "%f,%f,%f\n", &mag.magnetic_heading, &mag.pitch, &mag.roll);
              new_meas = true;
              break;
            }
          }
        }
      }
    }

    if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs) && new_meas) {
      currentStamp.stamp();
      if((currentStamp - last_mag_tx ) > (MAG_SEND_PERIOD * SEC2MICRO) ) {

        MagStruct_t * dataPtr = (MagStruct_t *)CMD_DATAPTR(fifo_msg.data);

        CMD_CMD((fifo_msg.data)) = CMD_MAG_TELEM;
        CMD_NUM((fifo_msg.data)) = sizeof(MagStruct_t);
        dataPtr->magnetic_heading = mag.magnetic_heading;
        dataPtr->pitch = mag.pitch;
        dataPtr->roll = mag.roll;
        CMD_CSUM(fifo_msg.data) = 0;
        CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

        fifo_msg.size = CMD_SIZE(fifo_msg.data);

        Client * a_client;
        Client * me = shmPtr->clients.getClient(0);
        int i=0;
        while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_MAG_TELEM,i)) != NULL)
				{
					fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me, a_client).s_addr;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
					i++;
				}

        last_mag_tx.stamp();
        new_meas = false;
      }
    }
  }
}
